#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程订阅/cmd_vel话题，发布/cmd_vel_new，消息类型geometry_msgs::Twist

import rospy
from geometry_msgs.msg import Twist

savior_vel_pub = rospy.Publisher('/cmd_vel_new', Twist, queue_size=5)
vel_oldold= Twist()
vel_oldold.linear.x=1
vel_oldold.linear.y=-0.1
vel_oldold.angular.z=-0.5
vel_old =vel_oldold

def save_CB(vel):
    #print(vel.linear.x,vel.linear.y,vel.angular.z)
    if -0.1<vel.linear.x<0.1 and -0.05<vel.linear.y<0.05 and -0.1<vel.angular.z<0.1:
        global vel_old
        vel_saved = Twist()
        if vel_old.linear.x==vel_old.linear.y==vel_old.angular.z:
            global vel_oldold
            vel_saved.linear.x = vel_oldold.linear.x *2/5
            vel_saved.linear.y = vel_oldold.linear.y *2/ 5
            vel_saved.angular.z = vel_oldold.angular.z *2 / 5
            rospy.loginfo("DoubleSave!!! x %f,y %f,a %f",
                  vel_saved.linear.x, vel_saved.linear.y,vel_saved.angular.z)
            savior_vel_pub.publish(vel_saved)
            #rospy.sleep(0.1)
            #savior_vel_pub.publish(vel_saved)
        else:
            vel_saved.linear.x = vel_old.linear.x *3/5
            vel_saved.linear.y = vel_old.linear.y *3/ 5
            vel_saved.angular.z = vel_old.angular.z *3 / 5
            rospy.loginfo("Save!!! x %f,y %f,a %f",
                  vel_saved.linear.x, vel_saved.linear.y,vel_saved.angular.z)
            savior_vel_pub.publish(vel_saved)
            #rospy.sleep(0.1)
            #savior_vel_pub.publish(vel_saved)
            vel_oldold= vel_old
    else:
        savior_vel_pub.publish(vel)
    vel_old= vel
    
def velocity_publisher():
    rospy.init_node('savior_node', anonymous=True)
    rospy.Subscriber("/cmd_vel", Twist, save_CB)
    rospy.spin()
    #rate = rospy.Rate(16)

    #while not rospy.is_shutdown():
        #rate.sleep()

if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

